#include "Ode23.h"

using namespace RSIM;

void Ode23::attemptSingleStep(void(*calcYDot)(const double& ,const Vector&,Vector&), // function f(t,y) = dy/dt
                                        const double& t0, 	  // given: t
                                        const Vector& Y0, 	  // given: Y(t)
                                        const Vector& Y0Dot,	  // given: yDot(t)
                                        const double& h, 	  // given: deltaT
                                        Vector& Y_deltaT, 	  // calculate:Y(t+deltaT)
                                        Vector& Y1Dot,	  	  // calculate:YDot(t+deltaT)
                                        double& error		  // calculate: estimate of local error in this step.
                        ){
        // Bogacki–Shampine method
        // http://en.wikipedia.org/wiki/Bogacki-Shampine_method

        const double C21   =  1.0/2.0;
        const double C22   =  1.0/2.0;

        const double C31   =  3.0/4.0;
        const double C33   =  3.0/4.0;	
        
        const double C42   =  2.0/9.0;
        const double C43   =  1.0/3.0;
        const double C44   =  4.0/9.0;

        const double CE1   = -5.0/72.0;
        const double CE2   = 1.0/12.0;
        const double CE3   = 1.0/9.0;
        const double CE4   = -1.0/8.0;	
        
        // new labels for intermediate terms
        const Vector& k0 = Y0Dot;
        Vector& k1 = k[0];
        Vector& k2 = k[1];	
        Vector& k3 = Y1Dot;
        
        // sFlag	
        calcYDot(t0+C21*h, Y0+(h*C22)*k0, k1);
        calcYDot(t0+C31*h, Y0+(h*C33)*k1, k2);		
        
        Y_deltaT = Y0+(h*C42)*k0+(h*C43)*k1+(h*C44)*k2;
        calcYDot(t0+h, Y_deltaT, k3);
        
        // sFlag
        this->estimateLocalStepError(Y0,
                                Y_deltaT,
                                CE1*k0+CE2*k1+CE3*k2+CE4*k3, 
                                h,
                                error);
}

void Ode23::interpolate(const double& t0, 
                        const Vector& y0, 
                        const Vector& y0Dot, 
                        const Vector& y1Dot, 
                        const double& h, 
                        const double& S1, 
                        Vector& Y){

        // See documentation for ntrp45.m of Matlab. This implementation is identical
        // to that of ntrp23.m
        const double B11 = 1.0;
        const double B12 = -4.0/3.0;
        const double B13 = 5.0/9.0;

        const double B22 = 1.0;
        const double B23 = -2.0/3.0;
        
        const double B32 = 4.0/3.0;
        const double B33 = -8.0/9.0;

        const double B42 = -1.0;
        const double B43 = 1.0;

        const double S2 = S1*S1;
        const double S3 = S1*S2;	
        
        const Vector& k0 = y0Dot;
        const Vector& k1 = k[0];
        const Vector& k2 = k[1];
        const Vector& k3 = y1Dot;
        
        //sFlag
        Y = y0 + (h*S1*B11)*k0;
        Y += (h*S2*B12)*k0 + (h*S2*B22)*k1 + (h*S2*B32)*k2 + (h*S2*B42)*k3;
        Y += (h*S3*B13)*k0 + (h*S3*B23)*k1 + (h*S3*B33)*k2 + (h*S3*B43)*k3;
}

void Ode23::initIntegrator(const Vector& Y){
        // allocate space for k
        for(int i=0;i<2;k[i++].resize(numY_));
        this->MinScaleFactor_ = 0.5;
}

// @book{hairer1993,
//   title={Solving ordinary differential equations: Nonstiff problems},
//   author={Hairer, E. and N{\o}rsett, S.P. and Wanner, G.},
//   year={1993},
//   publisher={Springer}
// }